#include <limits.h>

namespace Vtl
{
	namespace common
	{
		template<typename Scalar, int Dimension>
		inline Scalar distance2D( const Eigen::Matrix<Scalar,Dimension,1> &_P0, 
			const Eigen::Matrix<Scalar,Dimension,1> &_P1)
		{
			return sqrtl(sqDistance2D(_P0,_P1));
		}

		template<typename Scalar, int Dimension>
		inline Scalar sqDistance2D( const Eigen::Matrix<Scalar,Dimension,1> &_P0, 
			const Eigen::Matrix<Scalar,Dimension,1> &_P1)
		{
			typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
			VectorType diff = _P0 -_P1;
			Scalar i = diff(0);
			Scalar j = diff(1);
			return ((i*i) + (j*j));
		}

		template<typename Scalar, int Dimension>
		inline Scalar distance( const Eigen::Matrix<Scalar,Dimension,1> &_P0, 
			const Eigen::Matrix<Scalar,Dimension,1> &_P1)
		{
			return sqrtl(sqDistance(_P0,_P1));
		}

		template<typename Scalar, int Dimension>
		inline Scalar sqDistance( const Eigen::Matrix<Scalar,Dimension,1> &_P0, 
			const Eigen::Matrix<Scalar,Dimension,1> &_P1)
		{
			typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
			VectorType diff = _P0 -_P1;
			return diff.squaredNorm();
		}

		template <typename Scalar, int Dimension>
		inline Eigen::Matrix<Scalar, 2,1> convertTo2D(const Eigen::Matrix<Scalar, Dimension,1> &_Matrix)
		{
			return Eigen::Matrix<Scalar, 2,1>(_Matrix(0), _Matrix(1));
		}

		template <typename Scalar, int Dimension>
		inline Eigen::ParametrizedLine<Scalar, 2> convertTo2D(const Eigen::ParametrizedLine<Scalar, Dimension> &_Line)
		{
			return Eigen::ParametrizedLine<Scalar, 2>(convertTo2D(_Line.origin()), convertTo2D(_Line.direction()));
		}

		template <typename Scalar, int Dimension>
		inline Eigen::Matrix<Scalar, 3,1> convertTo3D(const Eigen::Matrix<Scalar, Dimension,1> &_Matrix, const Scalar &_K /*= 0.0*/)
		{
			if(Dimension<3)
			{
				return Eigen::Matrix<Scalar, 3,1>(_Matrix(0), _Matrix(1), _K);
			}
			return Eigen::Matrix<Scalar, 3,1>(_Matrix(0), _Matrix(1), _Matrix(2));
		}

		template <typename Scalar, int Dimension>
		inline Eigen::ParametrizedLine<Scalar, 3> convertTo3D(const Eigen::ParametrizedLine<Scalar, Dimension> &_Line, const Scalar &_K /*= 0.0*/)
		{
			return Eigen::ParametrizedLine<Scalar, 3>(convertTo3D(_Line.origin(), _K), convertTo3D(_Line.direction(), _K));
		}

		template <typename Scalar>
		inline Scalar random()
		{
			unsigned int number;
			rand_s(&number);
			return (Scalar(number)/(Scalar(UINT_MAX) + Scalar(1)));
		}

		template <typename Scalar>
		inline Scalar rangedRandom(const Scalar &_MinValue /*= Scalar(-1)*/, const Scalar &_MaxValue /*= Scalar(1)*/)
		{
			const Scalar scale_factor = _MaxValue - _MinValue;
			unsigned int number;
			rand_s(&number);
			return (_MinValue + (scale_factor * (Scalar(number)/Scalar(UINT_MAX))));
		}

		template <typename Scalar, int Dimension>
		inline Eigen::Matrix<Scalar, Dimension, 1> randomVector(const Scalar &_Scale /*= Scalar(1)*/)
		{
			Eigen::Matrix<Scalar, Dimension, 1> vector;
			for (int i = 0;i<Dimension;i++)
			{
				vector(i) = _Scale * rangedRandom<Scalar>();
			}
			return vector;
		}
	};
};